/*
 *  correctGamma.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-01-29.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "correctGamma.h"
#include <iostream>

void createGammaLUT( CvMat* gammaData, CvMat* gammaFit, CvMat** gammaLUT )
{
    unsigned int end, pixOut;
    double a, b, c;
    end=gammaData->rows;
    for (int i=1023; i>end; i=i-4)
    {
        a=cvmGet( gammaFit, 0, 0 );
        b=cvmGet( gammaFit, 1, 0 );
        c=cvmGet( gammaFit, 2, 0 );
        pixOut=cvRound((a*pow( (double) i, b )+c)/4);
        if ( pixOut < cvmGet(gammaData, end-1, 0)/4 )
            pixOut = (int)cvmGet(gammaData, end-1, 0)/4;
        else if ( pixOut > 255 )
            pixOut = 255;
    cvSet1D( *gammaLUT, (i+1)/4-1, cvScalar(pixOut) );
//        cvmSet( *gammaLUT, (i+1)/4-1, 0, pixOut ); //Convert from 1024 elements to 256
    }
    
    
    for (int i=0; i<=end; i=i+4)
    {
        pixOut = (int)cvmGet( gammaData, i/4, 0 )/4;
    cvSet1D( *gammaLUT, i/4, cvScalar(pixOut) );
//        cvmSet( *gammaLUT, i/4, 0, pixOut ); //Convert from 1024 elements to 256
    }

    return;
}

/*
unsigned int correctGamma(CvMat* gammaLUT, CvMat* gammaFit, unsigned int pixIn){
    unsigned int end, pixOut;
    pixIn=pixIn*4;
    end=gammaLUT->rows;
    if ( pixIn < cvmGet(gammaLUT, end, 0) ){
        float targetVal, middleVal, lowerVal, upperVal;
        unsigned int lowerIndex, upperIndex;
        lowerIndex=0;
        upperIndex=end-1;
        do{
            lowerVal =  cvmGet( gammaLUT, lowerIndex, 0 );
            upperVal =  cvmGet( gammaLUT, upperIndex, 0 );
            middleVal = cvmGet( gammaLUT, (lowerIndex+upperIndex)/2, 0 );
            
            if (pixIn > middleVal)
                lowerIndex=(unsigned int)floor((float)(lowerIndex+upperIndex)/2);
            else
                upperIndex=(unsigned int)ceil((float)(lowerIndex+upperIndex)/2);
            
        } while( (lowerVal != cvmGet(gammaLUT, lowerIndex, 0)) && (upperVal != cvmGet(gammaLUT, upperIndex, 0)) );
        cvmGet(gammaLUT, upperIndex, 1);
        cvmGet(gammaLUT, lowerIndex, 1);
        pixOut=cvRound(cvmGet(gammaLUT, lowerIndex, 1)+(cvmGet(gammaLUT, upperIndex, 1)-cvmGet(gammaLUT, lowerIndex, 1))/(upperVal-lowerVal)*(pixIn-lowerVal));
    }
    else
    {
        double a, b, c;
        a=cvmGet( gammaFit, 0, 0 );
        b=cvmGet( gammaFit, 1, 0 );
        c=cvmGet( gammaFit, 2, 0 );
        pixOut=cvRound(a*pow( (double) pixIn, b )+c);
        if ( pixOut > 1023)
            pixOut = 1023;
        else if ( pixOut < cvmGet(gammaLUT, end, 0))
            pixOut = cvmGet(gammaLUT, end, 0);
    } 
    return pixOut;
}
 */

/*
// Does a power-law transformation (gamma encoding) of an image.  The
// in_image and out_image arguments could refer to the same object, in
// which case the transformation is done in-place.  The input and output
// image types may differ, because a common use case is the
// gamma-encoding of a 10 or 12-bit image into 8 bits, and vice versa.
// The optional fs_out and fs_in arguments are doubles because some
// pixel types may be non-integer and have floating-point full-scale
// values.  Their default values are derived from the maxima of the
// pixel component types.  See
// http://www.w3.org/TR/PNG-GammaAppendix.html and
// http://www.poynton.com/notes/colour_and_gamma/GammaFAQ.html for some
// theory.

#include <limits>
#include <vector>

namespace image_gamma_impl  // Forward declaration of helper:
{ template <class Out_image_type, class In_image_type> class Gamma_image; }

template <class Out_image_type, class In_image_type>
Out_image_type& image_gamma(Out_image_type& out_image,
                           const In_image_type& in_image,
                           const double gamma,
                           const double fs_out = 0.0,
                           const double fs_in = 0.0)
{
    // Use a static member in helper class for partial specialization:
    return image_gamma_impl::Gamma_image<Out_image_type,
In_image_type>::gamma(
       out_image, in_image, gamma, fs_out, fs_in);
}  // image_gamma()
namespace image_gamma_impl
{

// Gamma (monochrome pixel types only):
template <>
class Gamma_image<image::Image<unsigned char>,
                 image::Image<unsigned char> >
{
private:
    typedef image::Image<unsigned char> Out_image_type;
    typedef image::Image<unsigned char> In_image_type;
    typedef Out_image_type::pixel_type Out_pixel_type;
    typedef In_image_type::pixel_type In_pixel_type;
    typedef In_image_type::size_type Isize_type;

public:

    static Out_image_type& gamma(Out_image_type& out_image,
                                const In_image_type& in_image,
                                const double g,
                                const double fs_out,
                                const double fs_in)
    {
       assert(out_image.rows()    == in_image.rows());
       assert(out_image.columns() == in_image.columns());
       assert(g > 0.0);
       assert(fs_out >= 0.0);
       assert(fs_in  >= 0.0);
       assert(fs_out <= std::numeric_limits<Out_pixel_type>::max());
       assert(fs_in  <= std::numeric_limits<In_pixel_type>::max());

       // Establish full-scale values:
       Out_pixel_type out_full_scale = Out_pixel_type(floor(fs_out + 0.5));
       if (fs_out == 0.0)  // Exactly 0.
           out_full_scale = std::numeric_limits<Out_pixel_type>::max();
       In_pixel_type in_full_scale = In_pixel_type(floor(fs_in + 0.5));
       if (fs_in == 0.0)  // Exactly 0.
           in_full_scale = std::numeric_limits<In_pixel_type>::max();

       // Build LUT (for monochrome pixel types only):
       size_t lut_size = size_t(in_full_scale) + 1;
       std::vector<Out_pixel_type> lut(lut_size);
       for (size_t i=0; i<lut_size; ++i)
       {
           double norm_val = pow(double(i)/double(in_full_scale), g);
           lut[i] = Out_pixel_type(floor(out_full_scale*norm_val + 0.5));
       }

       // Transform image:
       for (Isize_type r=0; r<in_image.rows(); ++r)
           for (Isize_type c=0; c<in_image.columns(); ++c)
               out_image(r,c) = lut[in_image(r,c)];

       return out_image;
    }  // gamma<Image<unsigned char>, Image<unsigned char> >()

};  // class Gamma_image<Image<unsigned char>, Image<unsigned char> >

}// namespace image_gamma_impl
 */
